﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace jKinect
{
    public class Smoother
    {
        private readonly Queue<Microsoft.Kinect.SkeletonPoint> _history = new Queue<Microsoft.Kinect.SkeletonPoint>();
        private readonly Queue<double> _weightedX = new Queue<double>();
        private readonly Queue<double> _weightedY = new Queue<double>();
        private readonly Queue<double> _weightedZ = new Queue<double>();

        private double ExponentialMovingAverage(double[] data, double baseValue)
        {
            double numerator = 0;
            double denominator = 0;

            double average = data.Sum();
            average /= data.Length;

            for (int i = 0; i < data.Length; ++i)
            {
                numerator += data[i] * Math.Pow(baseValue, data.Length - i - 1);
                denominator += Math.Pow(baseValue, data.Length - i - 1);
            }

            numerator += average * Math.Pow(baseValue, data.Length);
            denominator += Math.Pow(baseValue, data.Length);

            return numerator / denominator;
        }

        public Microsoft.Kinect.SkeletonPoint ExponentialWeightedAvg(Microsoft.Kinect.SkeletonPoint position, int smoothFactor)
        {
            _history.Enqueue(position);
            _weightedX.Enqueue(position.X);
            _weightedY.Enqueue(position.Y);
            _weightedZ.Enqueue(position.Z);

            while (_weightedX.Count > smoothFactor)
            {
                _history.Dequeue();
                _weightedX.Dequeue();
                _weightedY.Dequeue();
                _weightedZ.Dequeue();
            }
            float x = (float)ExponentialMovingAverage(_weightedX.ToArray(), 0.9);
            float y = (float)ExponentialMovingAverage(_weightedY.ToArray(), 0.9);
            float z = (float)ExponentialMovingAverage(_weightedZ.ToArray(), 0.9);

            return new Microsoft.Kinect.SkeletonPoint() { X = x, Y = y, Z = z };
        }

        public bool Any(Func<Microsoft.Kinect.SkeletonPoint, bool> func)
        {
            return _history.Any(func);
        }

        public bool All(Func<Microsoft.Kinect.SkeletonPoint, bool> func)
        {
            return _history.All(func);
        }
        public static float Scale(int maxPixel, float ratio, float position)
        {
            float num = (float)maxPixel / ratio / 2f * position + (float)(maxPixel / 2);
            if (num > (float)maxPixel)
            {
                return (float)maxPixel;
            }
            if (num < 0f)
            {
                return 0f;
            }
            return num;
        }

    }
}
